#from numpy import zeros, cos, sin, matrix, float
import numpy as np

def rotate_zyz(psi,theta,phi):
    Z = [[np.cos(psi), -np.sin(psi), 0.0],
         [np.sin(psi),  np.cos(psi), 0.0],
         [        0.0,          0.0, 1.0]]
    R1 = np.matrix(Z,np.float)

    Y = [[ np.cos(theta), 0.0, np.sin(theta)],
         [           0.0, 1.0,           0.0],
         [-np.sin(theta), 0.0, np.cos(theta)]]

    R2 = np.matrix(Y,np.float)

    Z = [[np.cos(phi), -np.sin(phi), 0.0],
         [np.sin(phi),  np.cos(phi), 0.0],
         [        0.0,          0.0, 1.0]]
    R3 = np.matrix(Z,np.float)

    return R3*R2*R1


def	create_projection_grid(N,K):
	M = np.zeros((N,N,K,K,3),np.float)
	stride = 1.0/float(K)

	M1 = np.zeros((K,K,3), np.float)
	for i in range(K):
		for j in range(K):
			M1[i,j,0] = (i + 0.5)*stride 
			M1[i,j,1] = (j + 0.5)*stride
			M1[i,j,2] = 0.0


	for i in range(N):
		for j in range(N):
			M[i,j,:,:] = [float(i),float(j),0.0]
			M[i,j] += M1

	return M


def create_projection_grids(N,K,radius):
	bot_grid = create_projection_grid(N,K)
	centre = 0.5*N
	radius2 = (centre*radius)**2

	for row in range(N):
		for col in range(N):
			M = bot_grid[row,col]
			for i in range(K):
				for j in range(K):
					x,y,z = M[i,j]
					x -= centre
					y -= centre
					rxy2 = x*x + y*y

					if rxy2 > radius2: M[i,j,:] = centre
					else: M[i,j,2] = centre - np.sqrt(radius2 - x*x - y*y)

	top_grid = bot_grid.copy()
	top_grid[:,:,:,:,2] = N - bot_grid[:,:,:,:,2]

	return bot_grid, top_grid
